#include "state_estimation/axis_trans.h"
#include "arm_math.h"

#ifndef PI
#	define PI 3.1415926f
#endif

void quat2angle(float *angle, float *quat)
{
	float c32;
	float c33;
	float c31;
	float c11;
	float c21;

	c32 = 2 * (quat[2] * quat[3] + quat[0] * quat[1]);
	c33 = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]);
	c31 = 2 * (quat[1] * quat[3] - quat[0] * quat[2]);
	c11 = 1 - 2 * (quat[2] * quat[2] + quat[3] * quat[3]);
	c21 = 2 * (quat[1] * quat[2] + quat[0] * quat[3]);

	angle[2] = atan2(c21, c11);
	angle[1] = asin(-c31);
	angle[0] = atan2(c32, c33);

	angle[2] = wrap_2pi(angle[2]);
}

void angle2quat(float *quat, float *angle)
{
	float cosFai = arm_cos_f32(angle[0]/2);
	float sinFai = arm_sin_f32(angle[0]/2);
	float cosTheta = arm_cos_f32(angle[1]/2);
	float sinTheta = arm_sin_f32(angle[1]/2);
	float cosPsi = arm_cos_f32(angle[2]/2);
	float sinPsi = arm_sin_f32(angle[2]/2);

	quat[0] = cosFai*cosTheta*cosPsi + sinFai*sinTheta*sinPsi;
	quat[1] = sinFai*cosTheta*cosPsi - cosFai*sinTheta*sinPsi;
	quat[2] = cosFai*sinTheta*cosPsi + sinFai*cosTheta*sinPsi;
	quat[3] = cosFai*cosTheta*sinPsi - sinFai*sinTheta*cosPsi;

	quat_normalize(quat);
}

void quat2cbn(float *cbn, float *quat)
{
	float a2 = quat[0]*quat[0];
	float b2 = quat[1]*quat[1];
	float c2 = quat[2]*quat[2];
	float d2 = quat[3]*quat[3];
	float ab = quat[0]*quat[1];
	float ac = quat[0]*quat[2];
	float ad = quat[0]*quat[3];
	float bc = quat[1]*quat[2];
	float bd = quat[1]*quat[3];
	float cd = quat[2]*quat[3];

	cbn[0] = a2+b2-c2-d2;
	cbn[1] = 2*(bc-ad);
	cbn[2] = 2*(bd+ac);
	cbn[3] = 2*(bc+ad);
	cbn[4] = a2-b2+c2-d2;
	cbn[5] = 2*(cd-ab);
	cbn[6] = 2*(bd-ac);
	cbn[7] = 2*(cd+ab);
	cbn[8] = a2-b2-c2+d2;
}

void quat_normalize(float *quat)
{
	float norm;
	arm_sqrt_f32(quat[0]*quat[0] + quat[1]*quat[1]
		     + quat[2]*quat[2] + quat[3]*quat[3], &norm);
	quat[0] /= norm;
	quat[1] /= norm;
	quat[2] /= norm;
	quat[3] /= norm;
}

void angle2cbn(float *cbn, float *angle)
{
	float quat[4];
	angle2quat(quat, angle);
	quat2cbn(cbn, quat);
}

void acc2angle(float *acc, float *angle)
{
	float norm;
	arm_sqrt_f32(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2], &norm);
	/* pitch angle */
	angle[1] = asin(acc[0]/norm);
	/* roll angle */
	angle[0] = atan2(-acc[1],-acc[2]);
	angle[2] = 0.0f;
}

void mag2angle(float *mag, float *angle)
{
	float Hx, Hy;
	float sinFi = arm_sin_f32(angle[0]);
	float cosFi = arm_cos_f32(angle[0]);
	float sinTh = arm_sin_f32(angle[1]);
	float cosTh = arm_cos_f32(angle[1]);

	Hx = mag[0]*cosTh + mag[1]*sinFi*sinTh + mag[2]*cosFi*sinTh;
	Hy = mag[1]*cosFi - mag[2]*sinFi;

	angle[2] = atan2(Hy, Hx);
	angle[2] = wrap_2pi(angle[2]);
}

float wrap_2pi(float angle)
{
	float res = angle;
	while (res > 2 * PI) {
		res -= 2 * PI;
	}
	while (res < 0.0) {
		res += 2 * PI;
	}
	return res;
}

float wrap_pi(float angle)
{
	float res = angle;
	while (res > PI) {
		res -= 2 * PI;
	}
	while (res < -PI) {
		res += 2 * PI;
	}
	return res;
}
